#ifndef FRENETOPTIMALTRAJECTORY_H
#define FRENETOPTIMALTRAJECTORY_H

#include <vector>
#include <cmath>
#include <queue>
#include <chrono>
#include <complex>
#include <iomanip>
#include <memory>
#include <ros/ros.h>
#include <algorithm>
#include <glog/logging.h>
#include "paths_struct.h"
#include "paths_routing_par.h"
#include "CubicSpline2D.h"
#include "FrenetPath.h"
#include "QuinticPolynomial.h"
#include "QuarticPolynomial.h"
#include "trajectoryPoint.h"


struct CostComparator_for_Frenet {
    bool operator()(const std::unique_ptr<FrenetPath>& left, const std::unique_ptr<FrenetPath>& right) const {
        return left->W > right->W;
    }
};

class FrenetOptimalTrajectory
{
public:
    FrenetOptimalTrajectory(/* args */);
    ~FrenetOptimalTrajectory();
    void calc_frenet_paths(FrenetInitialConditions* fot_ic, FrenetHyperparameters* fot_hp,
        std::priority_queue<std::unique_ptr<FrenetPath>, std::vector<std::unique_ptr<FrenetPath>>, CostComparator_for_Frenet>& frenet_path, 
        const std::vector<std::pair<double, double>>& global_points, const std::vector<double>& acc_s, 
        const int& index, const std::unique_ptr<CubicSpline2D>& Cub_s);

    bool to_global_path(std::unique_ptr<FrenetPath>& tfp, 
                                            const std::vector<std::pair<double, double>>& global_points,
                                            const std::vector<double>& acc_s, const int& index,
                                            const std::unique_ptr<CubicSpline2D>& Cub_s);
    bool is_valid_path(std::unique_ptr<FrenetPath>& tfp);
    DiscretizedTrajectory FrenetPath_to_TrajectoryPoint(std::unique_ptr<FrenetPath>& best_path);
private:
    /* data */
};

#endif // FRENETOPTIMALTRAJECTORY_H